package com.test.imu;

import android.app.Activity;
import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.test.MoreAsserts;
import android.util.Log;
import android.widget.RelativeLayout;
import android.widget.TextView;

public class MainActivity extends Activity implements SensorEventListener 
{
	private float mLastX, mLastY, mLastZ;
	private boolean mInitialized; 
	
	private SensorManager mSensorManager; 
	private Sensor mOrientation; 
	private Sensor mAcceleration;
	private Sensor mOther;

	float[] mGravity;
	float[] mGravity2;
	float[] mGeomagnetic;
	float[] mGeomagnetic2;
	
	private RotatableView view;
	
	private final float NOISE = 0.5f;
	private float[] rotationMatrix;
	
	@Override
	protected void onCreate(Bundle savedInstanceState) 
	{
		super.onCreate(savedInstanceState);
		setContentView(R.layout.activity_main);
		
		mInitialized = false;
		mSensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
		mOrientation = mSensorManager.getDefaultSensor(Sensor.TYPE_ORIENTATION);
		
		mAcceleration = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
		mOther = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
		
//		mSensorManager.registerListener(this, mOrientation, SensorManager.SENSOR_DELAY_NORMAL);

		RelativeLayout mainLayout = (RelativeLayout) findViewById(R.id.main_layout);		
		view = new RotatableView(this);
		
		mainLayout.addView(view);		
	}

	protected void onResume() 
	{
		super.onResume();
		mSensorManager.registerListener(this, mOrientation, SensorManager.SENSOR_DELAY_FASTEST);
		mSensorManager.registerListener(this, mAcceleration, SensorManager.SENSOR_DELAY_FASTEST);
		mSensorManager.registerListener(this, mOther, SensorManager.SENSOR_DELAY_FASTEST);
	}
	
	protected void onPause() 
	{
		super.onPause();
		mSensorManager.unregisterListener(this);
	}
	
	@Override
	public void onAccuracyChanged(Sensor sensor, int accuracy) {
		// TODO Auto-generated method stub
		
	}
	
	@Override
	public void onSensorChanged(SensorEvent event) 
	{		
		TextView tvX;
		TextView tvY;
		TextView tvZ;	
		
		tvX = (TextView)findViewById(R.id.oth_x_axis);
		tvY = (TextView)findViewById(R.id.oth_y_axis);
		tvZ = (TextView)findViewById(R.id.oth_z_axis); 	
		
		if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
		{
			mGravity = event.values;
			mGravity2 = event.values;
			mGravity2[0] = (mGravity2[0] * 1+event.values[0]) * 0.5f;
			mGravity2[1] = (mGravity2[1] * 1+event.values[1]) * 0.5f;
			mGravity2[2] = (mGravity2[2] * 1+event.values[2]) * 0.5f;
		}
		if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
		{
			mGeomagnetic = event.values;
			mGeomagnetic2 = event.values;
			mGeomagnetic2[0] = (mGeomagnetic2[0] * 2+event.values[0]) * 0.33334f;
			mGeomagnetic2[1] = (mGeomagnetic2[1] * 2+event.values[1]) * 0.33334f;
			mGeomagnetic2[2] = (mGeomagnetic2[2] * 2+event.values[2]) * 0.33334f;
		}
		if (mGravity != null && mGeomagnetic != null) 
		{
			float R[] = new float[9];
			float I[] = new float[9];
			rotationMatrix = new float[16];
			
			boolean success = SensorManager.getRotationMatrix(rotationMatrix, null, mGravity2, mGeomagnetic2);
			
//			boolean success = SensorManager.getRotationMatrix(R, I, mGravity, mGeomagnetic);
			if (success) 
			{
				//Sets the axis based on phone standing up
				SensorManager.remapCoordinateSystem(rotationMatrix, SensorManager.AXIS_Y, SensorManager.AXIS_MINUS_X, rotationMatrix);
			
				//orientation contains: azimut, pitch and roll
				float orientation[] = new float[3];
				
			    SensorManager.getOrientation(rotationMatrix, orientation);
			    float azimut = orientation[0]*360/(2*3.14159f); 
			    float pitch = orientation[1]*360/(2*3.14159f);
			    float roll = orientation[2]*360/(2*3.14159f);
			    
				tvX.setText(String.format("%d", (int) pitch));
				tvY.setText(String.format("%d", (int) roll));
				tvZ.setText(String.format("%d", (int) azimut));
			}
		}
//		TextView tvX;
//		TextView tvY;
//		TextView tvZ;		
//		
////		ImageView iv = (ImageView)findViewById(R.id.image);
//		
//		int sensorType = event.sensor.getType();
//
//		float x = event.values[0];
//		float y = event.values[1];
//		float z = event.values[2];
//		
//		if(sensorType == Sensor.TYPE_ORIENTATION)
//		{
//			tvX = (TextView)findViewById(R.id.x_axis);
//			tvY = (TextView)findViewById(R.id.y_axis);
//			tvZ = (TextView)findViewById(R.id.z_axis); 		
//		} else if(sensorType == Sensor.TYPE_ACCELEROMETER)
//		{
//			tvX = (TextView)findViewById(R.id.acc_x_axis);
//			tvY = (TextView)findViewById(R.id.acc_y_axis);
//			tvZ = (TextView)findViewById(R.id.acc_z_axis); 	
//			
//			float deltaX = Math.abs(mLastX - x);
//			float deltaY = Math.abs(mLastY - y);
//			float deltaZ = Math.abs(mLastZ - z);
////			if (deltaX < NOISE) deltaX = (float)0.0;
////			if (deltaY < NOISE) deltaY = (float)0.0;
////			if (deltaZ < NOISE) deltaZ = (float)0.0;
//			mLastX = x;
//			mLastY = y;
//			mLastZ = z;
//			
////			x = deltaX;
////			y = deltaY;
////			z = deltaZ;
//		} else
//		{
//			tvX = (TextView)findViewById(R.id.oth_x_axis);
//			tvY = (TextView)findViewById(R.id.oth_y_axis);
//			tvZ = (TextView)findViewById(R.id.oth_z_axis); 		
//		}
//
//		tvX.setText(String.format("%d", (int) x));
//		tvY.setText(String.format("%d", (int) y));
//		tvZ.setText(String.format("%d", (int) z));
//			
//		if(sensorType == Sensor.TYPE_ORIENTATION)
//			view.setRotation(x,y,z);
		
//		Log.v(RotatableView.TAG, "It makes no Sensor");
	}
}